/*  
 *  Nanjing JIYU Technology Co.,Ltd All rights reserved.
 *  Copyright (C) 2022 
 *  ==============================================================
 * 
 *  track.c 
 *    底盘循迹控制功能、转弯功能的实现
 *
 *  --------------------------------------------------------------
 *  
 */

#include "track.h"

#include "device.h"


/* 前进循迹 */
void track_drive(track_speed_t s)
{
  motor_move(s, s* 80/100 * gray_front_error());
}

/* 倒车循迹，前后灰度err定义相同，差速值加个负号，逻辑相反 */
void track_reverse(track_speed_t s)
{
  motor_move(-s, -s* 80/100 * gray_back_error());
}

void track_stop(void)
{
  motor_move(0, 0);
}

/* 停在十字路口时 左转 */
void track_turn_left(track_speed_t s)
{
  motor_move(0, -s);

  delay_ms(500);

  while (gray_turn_not_ok0());  /* 没转到位，程序停在此处 */

  motor_move(0, 0);
}

void track_turn_right(track_speed_t s)
{
  motor_move(0, s);

  delay_ms(500);

  while (gray_turn_not_ok0());  /* 没转到位，程序停在此处 */

  motor_move(0, 0);
}


/* 循迹测试函数 */
void track_drive_test(void)
{
  while (1)
  {
    /* 20ms执行一次控制 */
    delay_ms(20);

    /* 车前进速度和差速值控制，error前的系数调整转弯力度 */
//    motor_move(300, 1000 * gray_front_error());
    motor_move(300, 260 * gray_front_error());

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  while (1);   /* 测试程序停在此处 */
}


void track_reverse_test(void)
{
  while (1)
  {
    delay_ms(20);

    track_reverse(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  while (1);   /* 测试程序停在此处 */
}


void track_turn_test(void)
{
  while (1)
  {
    /* 20ms执行一次控制 */
    delay_ms(20);

    /* 车前进速度和差速值控制，error前的系数调整转弯力度 */
    motor_move(300, 260 * gray_front_error());

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  delay_ms(500);

  track_turn_left(speed_medium);

  while (1);
}


